#ifndef _JOINT_H_
#define _JOINT_H_

#define     MOTOR_TYPE        "EC40"
#define     HIP_GEAR		  80	
#define     KEE_GEAR		  80		
#define     ENCODER_LINE      4096
#define     PI	              3.1415926
#define     HIP_RATIO         80
#define	    KNEE_RATIO        80
//4096*80/2*3.141
#define	    HIP_RAD_RATIO	  52151.8926//(HIP_DECELERATE*ENCODER_LINE_NUMBER)/(2.0*PI)	
#define	    KNEE_RAD_RATIO	  52151.8926 //(KNEE_DECELERATE*ENCODER_LINE_NUMBER)/(2.0*PI)		//83813.03	
//4096*80/360
#define     HIP_DEGREE_RATIO   910.2222 //(HIP_DECELERATE*ENCODER_LINE_NUMBER)/360.0	//2146.0
#define	    KNEE_DEGREE_RATIO  910.2222 //(KNEE_DECELERATE*ENCODER_LINE_NUMBER)/360.0 	//1462.8	

#define	    DEGREE_TO_RADIAN	0.01745	//PI/180
#define	    RADIAN_TO_DEGREE	57.295779	//PI/180


enum joint_id
{
    IDEL_ID = 0,
    LEFT_HIP = 1 ,
    LEFT_KNEE,  
    RIGHT_HIP,
    RIGHT_KNEE,
    JOINT_NUMBER
};
// enum joint_mode
// {
//     TORQUE,
//     VELOCITY,
//     POSITION ,
//     IDLE
// }

//offset of motor: the abs orginal angle when virtual axis is 0
#define LEFT_HIP_OFFSET   0.0 
#define LEFT_KNEE_OFFSET  0.0
#define RIGHT_HIP_OFFSET  0.0
#define RIGHT_KNEE_OFFSET 0.0

#define POSITIVE          1.0
#define NAGTIVE           -1.0
#define NORMAL_TORQUE     0.169
#define HIP_UPPER_LIMIT   15.0
#define HIP_LOWER_LIMIT   -90.0
#define KNEE_UPPER_LIMIT  90.0
#define KNEE_LOWER_LIMIT  0.0

void  joint_init(enum joint_id id);
float joint_get_angle(void);
float joint_get_velocity(void);
float joint_get_torque(void);
void  joint_data_update(void);
void  joint_servo_update(void);
void  set_joint_aim_angle(float degree);
void set_joint_servo_on(void);
void set_joint_servo_off(void);
#endif

